#include "clkMath.h"

V2DF nrLnPoint(V2DF a, V2DF b, V2DF p, bool seg)
{
	V2DF ab = b-a;
	V2DF ap = p-a;
	float len = ab.magnitudeSq();
	float dot = ab.dot(ap);

	if(len == 0.0f || dot == 0.0f)
		return V2D_ZERO;

	float t = dot*(1.0f/len);

	if(seg)
	{
		if(t < 0.0f) t = 0.0f;
		else if(t > 1.0f) t = 1.0f;
	}

	V2DF proj = ab * t;
	return a + proj;
}

bool intersectL2L(V2DF a, V2DF b, V2DF c, V2DF d, V2DF* p)
{
	V2DF ab = b-a;
	V2DF cd = d-c;
	V2DF ca = a-c;

	float den = ab.cross(cd);
	if(den == 0) return false;

	float t = cd.cross(ca)*(1.0f/den);
	float s = ab.cross(ca)*(1.0f/den);

	if(t < 0.0f || t > 1.0f) return false;
	if(s < 0.0f || s > 1.0f) return false;

	if(p != 0)
		*p = a+(ab*t);

	return true;
}

int random()
{
	static unsigned int nSeed = 5323;
	nSeed = (8253729 * nSeed + 2396403);
	return nSeed  & 0x7fffffff;
}

int randomInt()
{
	return random();
}

int randomInt(int min, int max)
{
	return (random()%(max-min))+min;
}

float randomFloat()
{
	int i = random() & 0xffff;
	return i / (float)(0x10000);
}

float randomFloat(float min, float max)
{
	float delta = max-min;
	float number = randomFloat()*delta;
	number += min;
	return number;
}

float polarAngle(V2DF cart)
{
	return atan2(cart.y,cart.x);
}

void clamp(float& value, float min, float max)
{
	if(value < min)
		value = min;
	else if(value > max)
		value = max;
}

void clamp(V2DF& value, float min, float max)
{
	float mag = value.magnitude();
	if(mag < min)
	{
		value.normalize();
		value *= min;
	}
	else if(mag > max)
	{
		value.normalize();
		value *= max;
	}
}

void clampDegree(float& value)
{
	if(value < 0.0f)
		value += 360.0f;
	if(value > 360.0f)
		value -= 360.0f;
}

void rotateVec(V2DF& vec, float dgr)
{
	float c = cos(RADIAN(dgr));
	float s = sin(RADIAN(dgr));

	V2DF v(vec.x*c-vec.y*s,vec.y*c+vec.x*s);
	vec.set(v.x,v.y);
}

void translateVec(V2DF& vec, V2DF trn)
{
	vec += trn;
}

V2DF centeroid(V2DF* verts)
{
	V2DF min, max;
	genMinMax(min,max,verts);
	return (min-max)/2.0f;
}

void genMinMax(V2DF& min, V2DF& max, float width, float height)
{
	float hw = width/2.0f;
	float hh = height/2.0f;

	min.set(-hw,-hh);
	max.set(hw,hh);
}

void genMinMax(V2DF& min, V2DF& max, V2DF* verts)
{
	min = verts[0]; max = verts[0];
	for(int i = 1; i < 4; i++)
	{
		min.x = (verts[i].x<min.x)?verts[i].x:min.x;
		min.y = (verts[i].y<min.y)?verts[i].y:min.y;
		max.x = (verts[i].x>max.x)?verts[i].x:max.x;
		max.y = (verts[i].y>max.y)?verts[i].y:max.y;
	}
}

clkAABB genAABB(V2DF pos, float width, float height)
{
	V2DF min, max;
	genMinMax(min,max,width,height);

	clkAABB aabb;
	aabb.m_cen = pos;
	aabb.m_min = pos+min;
	aabb.m_max = pos+max;

	return aabb;
}

clkAABB genAABB(V2DF pos, V2DF* verts)
{
	V2DF min, max;
	genMinMax(min,max,verts);

	clkAABB aabb;
	aabb.m_cen = pos;
	aabb.m_min = pos+min;
	aabb.m_max = pos+max;

	return aabb;
}

clkAABB genAABB(V2DF* verts)
{
	V2DF min, max;
	genMinMax(min,max,verts);

	clkAABB aabb;
	aabb.m_min = min;
	aabb.m_max = max;

	return aabb;
}

clkTriCone genTriCone(V2DF pos, V2DF dir, float width, float radius)
{
	V2DF perp = dir.perp();
	V2DF sideA = (perp * width)  + (dir * radius);
	V2DF sideB = (perp * -width) + (dir * radius);

	clkTriCone tri;
	tri.m_radius  = radius;
	tri.m_vert[0] = pos;
	tri.m_vert[1] = pos + sideA;
	tri.m_vert[2] = pos + sideB;
	tri.m_norm[0] = (tri.m_vert[0]-tri.m_vert[1]).perp().normal();
	tri.m_norm[1] = (tri.m_vert[1]-tri.m_vert[2]).perp().normal();
	tri.m_norm[2] = (tri.m_vert[2]-tri.m_vert[0]).perp().normal();

	return tri;
}

bool overlapAABB(clkAABB abA, clkAABB abB)
{
	if(abA.m_max.x < abB.m_min.x ||
		abA.m_min.x > abB.m_max.x ||
		abA.m_max.y < abB.m_min.y ||
		abA.m_min.y > abB.m_max.y)
		return false;
	return true;
}

bool pointInAABB(V2DF pnt, V2DF min, V2DF max)
{
	if(pnt.x < min.x ||
		pnt.x > max.x ||
		pnt.y < min.y ||
		pnt.y > max.y)
		return false;
	return true;
}

void calcPhysics(V2DF& pos, float& rot, V2DF& lvel, float& avel,
	V2DF lfrc, float afrc, float ldamp, float adamp,
	float mass, float inertia, float time)
{
	lvel += (lfrc * (1.0f/mass)) * time;
	lvel *= (1.0f - time * ldamp);

	avel += (afrc * (1.0f/inertia)) * time;
	avel *= (1.0f - time * adamp);

	pos += lvel * time;
	rot += avel * time;
}

satProj::satProj()
{
	minValue = 0;
	maxValue = 0;
}

satProj::satProj(float mn, float mx)
{
	minValue = mn;
	maxValue = mx;
}

bool satProj::overlap(satProj prj, float* ovr)
{
	if(minValue < prj.maxValue
		&& maxValue >  prj.minValue)
	{
		if(ovr != 0)
			*ovr = abs(minValue - prj.maxValue);
		return true;
	}
	else if(maxValue > prj.minValue
		&& minValue <  prj.maxValue)
	{
		if(ovr != 0)
			*ovr = abs(maxValue - prj.minValue);
		return true;
	}
	if(ovr != 0) *ovr = 0.0f;
	return false;
}

satProj satProject(V2DF* verts, V2DF axis, int size)
{
	float min = axis.dot(verts[0]);
	float max = min;
	for(int i = 1; i < size; i++)
	{
		float dot = axis.dot(verts[i]);
		if(dot < min) min = dot;
		else if(dot > max) max = dot;
	}

	return satProj(min,max);
}

bool satCollision(V2DF* vertsA, V2DF* normsA, V2DF* vertsB,
	V2DF* normsB, V2DF* mtvAxis, float* mtvMag)
{
	V2DF mtvAxisTemp;
	float mtvMagTemp = FLOAT_MAX;

	for(int i = 0; i < 4; i++)
	{
		satProj pA = satProject(vertsA,normsA[i]);
		satProj pB = satProject(vertsB,normsA[i]);

		float ovr = 0;
		if(!pA.overlap(pB,&ovr))
			return false;
		else
		{
			if(ovr < mtvMagTemp)
			{
				mtvMagTemp = ovr;
				mtvAxisTemp = normsA[i];
			}
		}
	}

	for(int i = 0; i < 4; i++)
	{
		satProj pA = satProject(vertsA,normsB[i]);
		satProj pB = satProject(vertsB,normsB[i]);

		float ovr = 0;
		if(!pA.overlap(pB,&ovr))
			return false;
		else
		{
			if(ovr < mtvMagTemp)
			{
				mtvMagTemp = ovr;
				mtvAxisTemp = normsB[i];
			}
		}
	}

	if(mtvAxis != 0) *mtvAxis = mtvAxisTemp;
	if(mtvMag != 0) *mtvMag = mtvMagTemp;

	return true;
}

bool satCollision(V2DF* vertsA, V2DF* normsA, int countA,
	V2DF* vertsB, V2DF* normsB, int countB)
{
	V2DF mtvAxisTemp;
	float mtvMagTemp = FLOAT_MAX;

	for(int i = 0; i < countA; i++)
	{
		satProj pA = satProject(vertsA,normsA[i],countA);
		satProj pB = satProject(vertsB,normsA[i],countB);

		float ovr = 0;
		if(!pA.overlap(pB,&ovr))
			return false;
		else
		{
			if(ovr < mtvMagTemp)
			{
				mtvMagTemp = ovr;
				mtvAxisTemp = normsA[i];
			}
		}
	}

	for(int i = 0; i < countB; i++)
	{
		satProj pA = satProject(vertsA,normsB[i],countA);
		satProj pB = satProject(vertsB,normsB[i],countB);

		float ovr = 0;
		if(!pA.overlap(pB,&ovr))
			return false;
		else
		{
			if(ovr < mtvMagTemp)
			{
				mtvMagTemp = ovr;
				mtvAxisTemp = normsB[i];
			}
		}
	}

	return true;
}